/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file gpsParser.cxx
  \brief class implemenation for gpsParser

  \author Jack Elston
  $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>

#include <dirent.h>

#include "rssiParser.h"

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---Global Variables---->*/
extern bool verbose;
extern bool logging;
extern String logDir;
/*<-End Global Variables-->*/

RSSIParser::RSSIParser(String name) : ThreadedPipe(name)
{
	buf_ptr = 0;
}

static int fileSelect(const struct dirent *entry)
{
	if (strncmp(entry->d_name, "rssi_", 4) == 0)
		return 1;
	else
		return 0;
}

bool RSSIParser::init(ShmStruct * shmStructPtr)
{
	shmPtr = shmStructPtr;
	mtx = shmPtr->mtx;

	last_rssi_tx.stamp();

	/*--------- log ----------*/
	if(logging) {
		struct dirent **files;
		int num = scandir(logDir.c_str(), &files, fileSelect, 0);
		if (num < 0) {
			fprintf(stderr,"Unable to open log file.\n");
			return 0;
		}
		else {
			fprintf(stderr,"using rssi_%03d.log\n",num);
			sprintf(log_name,"%s/rssi_%03d.txt",logDir.c_str(),num);
			while (num--) {free(files[num]);}
		}
		free(files);
	}
	/*--------- log ----------*/

	return true;
}

void RSSIParser::update() 
{
	int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;

	FD_ZERO(&readFDs);	                    		// zero read fd set
	FD_ZERO(&writeFDs);	                    		// zero write fd set
	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[OUTBOARD_RX],&readFDs);			// add fifo
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);			// add fifo
	FD_SET(fifo_FDs[OUTBOARD_TX],&writeFDs);		// add fifo

	fifo_FDs[INBOARD_RX] > fifo_FDs[OUTBOARD_RX] ? maxFD = fifo_FDs[INBOARD_RX] : maxFD = fifo_FDs[OUTBOARD_RX];
	if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];
	if(fifo_FDs[OUTBOARD_TX] > maxFD) maxFD = fifo_FDs[OUTBOARD_TX];

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	Client * me = shmPtr->clients.getClient(0);

	if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("RSSIParser::update() select");
				execRate = -1;
			} else
				cout << getName() << "::update() - select interrupted" << endl;
		}
	}
	else {
		if(FD_ISSET(fifo_FDs[OUTBOARD_RX], &readFDs))
		{
			int n = 0;
			n=readFifo(OUTBOARD_RX);
			if(n>0) {
				if(strncmp(fifo_msg.data,"Click::ControlSocket",20) != 0){
					int status = 0;
					sscanf(fifo_msg.data,"%i",&status);
					if(status == 200) {

						char * line;
						line = strtok(fifo_msg.data, "\n");
						line = strtok(NULL, "\n");

						char mac[17];
						char ip[15];
						int signal = 0;
						float last_rx = 0;

						int retval = 0;
						int linenum = 0;

						char * dataPtr = CMD_DATAPTR(fifo_msg.data);

						//cout << "(" << me->telemetry.pos.lat << "," << me-> telemetry.pos.lon << ")" << endl;
						//
						if(logging) {
							logfile=fopen(log_name,"a");
							if(logfile != NULL)
								fprintf(logfile,"(%f,%f,%f)\n",me->telemetry.gps.pos.lat,me->telemetry.gps.pos.lon,me->telemetry.airdata.alt);
						}

						LinkStatus_t * link_status;
						int payload_size=0;

						line = strtok(NULL, "\n");

						while ( line != NULL ) {
							retval = sscanf(line,"%s %s rate %*i signal %i noise %*i avg_signal %*f avg_noise %*f total_signal %*i total_noise %*i packets %*i last_received %f", mac, ip, &signal, &last_rx);

							if(retval == 4) {

								link_status = &((LinkStatus_t *)(dataPtr))[linenum];
								strncpy(link_status->mac,mac,17);
								link_status->rssi = signal;
								link_status->last_rx = last_rx;
								payload_size += sizeof(LinkStatus_t);
								linenum ++;

								//cout << " " << mac << " " << ip << " " << signal << " " << last_rx << endl;
								if(logging) {
									if(logfile != NULL)
										fprintf(logfile,"%s %s %i %f\n",mac,ip,signal,last_rx);
								}
							}
							line = strtok(NULL, "\n");
						}
						CMD_CMD((fifo_msg.data)) = CMD_LINK_STATUS;
						CMD_NUM((fifo_msg.data)) = payload_size;
						CMD_CSUM(fifo_msg.data) = 0;
						CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

						fifo_msg.size = CMD_SIZE(fifo_msg.data);

						Client * a_client;
						int i=0;
						while((a_client = shmPtr->clients.getSubscriptionsTo(CMD_LINK_STATUS,i)) != NULL)
						{
							fifo_msg.src.s_addr = shmPtr->clients.getAddr(me,a_client).s_addr;
							fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
							writeFifo(INBOARD_TX);
							i++;
						}

						//cout << endl;
						if(logging) {
							if(logfile != NULL) {
								fprintf(logfile,"\n");
								fclose(logfile);
							}
						}
					}
				}
			}
		}
		if(FD_ISSET(fifo_FDs[INBOARD_TX], &writeFDs)) {
			currentStamp.stamp();
			if((currentStamp - last_rssi_tx ) > (RSSI_PERIOD * SEC2MICRO) ) {

				fifo_msg.src.s_addr = shmPtr->clients.getAddr(me,a_client).s_addr;
				fifo_msg.dest.s_addr = inet_addr("127.0.0.1");
				strcpy(fifo_msg.data, "read rx.stats\n");
				fifo_msg.size = 14;
				writeFifo(OUTBOARD_TX);

				/*Client * a_client;
				  int i=0;
				  while((a_client = shmPtr->clients.getSubscriptionsTo(CMD_TELEM,i)) != NULL)
				  {
				  Client * me = shmPtr->clients.getClient(0);
				  fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;
				  fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
				  writeFifo(INBOARD_TX);
				  i++;
				  }*/
				last_rssi_tx.stamp();
			}
		}
	}
}

RSSIParser::~RSSIParser()
{
}
